#!usr/bin/env python
# -*- coding:utf-8 _*-
"""
@Time   :   2021/10/19  上午9:59
@Author :   HaoWANG, Foshan，China
@Email  :   haowanghk@163.com
@file   :  testing_demo.py.py
@Software : Pycharm
@Description:
"""

import uservo
import serial
import time

from MotionGroup import MotionGroup
from ServoPose import Pose
import logging

from wrist_gripper import WristGripperControl

from robot_arm import ArmMotionControl

SHORT_TIME_DELAY = 0.02  # time delay para
MEDIA_TIME_DELAY = 0.10
LONG_TIME_DELAY = 1.0

VEL = {'Very_low_spped': 10, 'LOW_SPEED': 15, 'MID_SPEED': 45, 'HIGH_SPEED': 60}
CONTROL_MODE = {'Teach': True, 'Repeat': False}

# DUAL_ARM_INIT_POSE = [140.0, 50.0, 70.0, 0, 2.0, -76.5, 60.0, 0, 0, 0]

DUAL_ARM_START_POSE = [140.0, 50.0, -32.0, 85, 2, -82, -35, 90, 60, 60]
DUAL_ARM_TARGET_POSE = [140.0, 50.0, 83.0, 90, 2, -82, -35, 90, 60, 60]

ending_servo_idx = [0, 1, 2, 3]


# 增加关节舵机角度的vel和iterval参数动态调整接口API
def generate_pose_vtc(angle, servo_id, vel=100, iterval=5000, t_acc=35, t_dec=35, power=10000, mean_dps=70.0):
    pose_tmp = {'angle': angle, 'is_mturn': False, 'interval': iterval,
                'vel': vel, 't_acc': t_acc,
                't_dec': t_dec, 'power': power,
                'mean_dps': mean_dps}
    gen_pose = Pose(POSE=pose_tmp, SERVO_ID=servo_id)
    return gen_pose


def main():
    pose_sd = {'Servo_id': 0, 'angle': None, 'is_mturn': False, 'interval': None, 'velocity': 10, 't_acc': 20,
               't_dec': 20,
               'power': 5000, 'mean_dps': 50.0}

    # dual_arm
    dual_arm = ArmMotionControl(arm_name="dual_arm", dof=6, CONTROL_MODE="DAMPING", with_wrist=False, is_debug=True,
                                damping_power=1000)
    wrist_gripper = WristGripperControl(servo_channel=ending_servo_idx, is_debug=True)

    # 初始化舵机管理器类对象、串口端口地址
    dual_arm.creat_uservo_obj(SERVO_BAUDRATE=115200)
    if dual_arm.servo_buffer(is_scan_servo=True):
        dual_arm.logging_arm_info()
    dual_arm_servo = dual_arm.get_arm_servo_list()

    # dual arm and gripper init
    dual_arm.arm_state_init(force_angle_init=False)

    dual_arm.set_all_servo_mode(CONTROL_MODE="CIRCLE")
    wrist_gripper.all_ending_init(motion_group=4)
    time.sleep(3)

    while True:
        keyInput = input('waitKey input (c or x):')
        if keyInput == 'x':
            dual_arm.set_all_servo_mode(CONTROL_MODE="CIRCLE")
            logging.warning('Interrupt:  The waitKey input (c or x): {}'.format(keyInput))
            break
        elif keyInput == 'c':
            # dual_arm再现
            wrist_gripper.set_servo_angle(servo_idx=0, angle=DUAL_ARM_START_POSE[3])
            # wrist_gripper.set_servo_angle(servo_idx=1, angle=DUAL_ARM_START_POSE[8])
            time.sleep(LONG_TIME_DELAY)
            # 右臂手腕手爪位置伺服，2（IO12）-》it-7，3(IO13)->it-[9]
            wrist_gripper.set_servo_angle(servo_idx=2, angle=DUAL_ARM_START_POSE[7])
            # wrist_gripper.set_servo_angle(servo_idx=3, angle=DUAL_ARM_START_POSE[9])
            time.sleep(3 * LONG_TIME_DELAY)

            # start pose trajectory control
            for SERVO_ID in dual_arm_servo:
                dual_arm.set_single_servo_pose(
                    Pose=generate_pose_vtc(angle=DUAL_ARM_START_POSE[SERVO_ID], vel=VEL.get('Very_LOW_SPEED'),
                                           iterval=5000,
                                           t_acc=30,
                                           t_dec=30, power=10000, mean_dps=30, servo_id=SERVO_ID))
            time.sleep(8*LONG_TIME_DELAY)

            # target pose trajectory control
            for SERVO_ID in dual_arm_servo:
                dual_arm.set_single_servo_pose(
                    Pose=generate_pose_vtc(angle=DUAL_ARM_TARGET_POSE[SERVO_ID], vel=VEL.get('Very_LOW_SPEED'),
                                           iterval=5000,
                                           t_acc=30,
                                           t_dec=30, power=10000, mean_dps=30, servo_id=SERVO_ID))

            # recoding the angle data and the power data
            with open('angle_power_time_200g.txt', 'w') as f:
                for frame in range(200):
                # data file write as txt
                    f.write("{}  {}  {}".format(frame,
                                                dual_arm.read_single_servo_angle(SERVO_ID=2) - DUAL_ARM_START_POSE[2],
                                                dual_arm.uservo_obj.query_power(servo_id=2)) + '\r')
                    time.sleep(0.5*MEDIA_TIME_DELAY)
            time.sleep(2 * LONG_TIME_DELAY)
        else:
            logging.warning('waitKey input (c or x): {}'.format(keyInput))


# main function
if __name__ == '__main__':
    # 创建一个logger_debug
    import sys

    logger = logging.getLogger()
    logger.setLevel(logging.DEBUG)  # Log等级总开关-debug
    print("Logging Set Info: logger.setLevel(logging.DEBUG)  # Log等级总开关-debug")
    try:
        main()
    except KeyboardInterrupt:
        print("Shutting down robot arm debug.")
